#include "Header.h"

float angle_now[7];
float angle_target[7];

/*设置舵机角度*/
void Servo_SetAngle(uint8_t id, float angle)
{
    int16_t compare;
    if (id == 2)
    {
        compare = 1500 + 100 + 2000 * angle / 180.0;
    }
    else if (id == 3)
    {
        compare = 1500 - 111 + 2000 * angle / 180.0;
    }
    else
    {
        compare = 1500 + 2000.0 * angle / 180.0;
    }
    if(compare < 500) compare = 500;
    if(compare > 2500) compare = 2500;
    PWM_SetCompare(id, compare);
}

/*舵机初始化*/
void Servo_Init(void)
{
    PWM_Init();
    angle_target[1] = 0;
    angle_target[2] = 0;
    angle_target[3] = 0;
    angle_target[4] = 0;
    angle_target[5] = 0;
    angle_target[6] = -60;
    angle_now[1] = 0;
    angle_now[2] = 0;
    angle_now[3] = 0;
    angle_now[4] = 0;
    angle_now[5] = 0;
    angle_now[6] = -60;
    for (int i = 1; i <= 6; i++)
    {
        Servo_SetAngle(i, angle_target[i]);
        Delay_ms(500);
    }
}
